﻿ /**
  * MOTOR 2 - RAPID FLASH DYNAMICS
  * Copyright (c) 2006-2008 Michael Baczynski http://www.polygonal.de
  *
  * This software is provided 'as-is', without any express or implied
  * warranty.  In no event will the authors be held liable for any damages
  * arising from the use of this software.
  * 
  * Permission is granted to anyone to use this software for any purpose,
  * including commercial applications, and to alter it and redistribute it
  * freely, subject to the following restrictions:
  * 
  * 1. The origin of this software must not be misrepresented; you must not
  * claim that you wrote the original software. If you use this software
  * in a product, an acknowledgment in the product documentation would be
  * appreciated but is not required.
  * 2. Altered source versions must be plainly marked as such, and must not be
  * misrepresented as being the original software.
  * 3. This notice may not be removed or altered from any source distribution.
  */
package de.polygonal.motor2.dynamics.contact.generator
{
	import de.polygonal.motor2.collision.shapes.ShapeSkeleton;
	
	
	import de.polygonal.motor2.collision.pairwise.CollidePoly;
	
	import de.polygonal.motor2.dynamics.contact.Manifold;
	import de.polygonal.motor2.dynamics.contact.Contact;
	import de.polygonal.motor2.dynamics.contact.ContactPoint;
	
	import de.polygonal.motor2.math.V2;
	
	public class PolyContact extends Contact
	{	
		public var manifold:Manifold;
		
		private var m0Cp1:ContactPoint, m1Cp1:ContactPoint;
		private var m0Cp2:ContactPoint, m1Cp2:ContactPoint;
		private var m0:Manifold;
		
		/* separation axis hint */
		public var firstShapeExit:Boolean;
		public var p:V2;
		public var n:V2;
		
//		public var p0:V2, d0:V2;
//		public var p1:V2, d1:V2;
		
		public static function create(shape1:ShapeSkeleton, shape2:ShapeSkeleton):Contact
		{
			return new PolyContact(shape1, shape2);
		}
		
		public static function destroy():void
		{
			//TODO optimize: recycle used manifold
		}
		
		public function PolyContact(shape1:ShapeSkeleton, shape2:ShapeSkeleton)
		{
			super(shape1, shape2);
			//TODO optimize: create manifold buffer queue
			manifold = manifolds[0] = new Manifold(); 
			m0 = new Manifold();
			
			m0Cp1 = m0.c0; m1Cp1 = manifold.c0;
			m0Cp2 = m0.c1; m1Cp2 = manifold.c1;
			
			_collider = new CollidePoly();
			
			/* separation axis hint */
			firstShapeExit = true;
			
//			p0 = shape1.worldVertexChain;
//			d0 = shape1.worldNormalChain.prev;
			
//			p1 = shape2.worldVertexChain;
//			d1 = shape2.worldNormalChain.prev;
			
			p = shape1.worldVertexChain;
			n = shape1.worldNormalChain.prev;
		}
		
		override public function evaluate():void
		{
			if (!shape1.synced) shape1.toWorldSpace();
			if (!shape2.synced) shape2.toWorldSpace();
			
			var k0:int, k1:int;
			
			//{store impulses from previous time step
			if (_doWarmStart)
			{
				manifold.c0.matched = false;
				manifold.c1.matched = false;
				
				k0 = manifold.pointCount;
				
				m0.pointCount = k0;
				if (k0 > 0)
				{
					m0Cp1.id = m1Cp1.id.copy();
					m0Cp1.Pn = m1Cp1.Pn;
					m0Cp1.Pt = m1Cp1.Pt;
					if (k0 > 1)
					{
						m0Cp2.id = m1Cp2.id.copy();
						m0Cp2.Pn = m1Cp2.Pn;
						m0Cp2.Pt = m1Cp2.Pt;
					}
				}
			}
			//}
			
			//{create contact manifold 
//			CollidePoly.collide(manifold, shape1, shape2, this);
			_collider.collide(manifold, shape1, shape2, this);
			
			if (manifold.pointCount > 0)
				manifoldCount = 1;
			else
			{
				manifoldCount = 0;
				return;
			}
			//}
			
			//{try to reapply impulses if contact id's match
			if (_doWarmStart)
			{
				m1Cp1.Pn = .0;
				m1Cp1.Pt = .0;
				m1Cp2.Pn = .0;
				m1Cp2.Pt = .0;
				
				k1 = manifold.pointCount;
				if (k1 == 1)
				{
					if (k0 == 1)
					{
						/* match one new contact against one old contact */
						
						//compare new1 -> old1
						if (m1Cp1.id.key == m0Cp1.id.key)
						{
							m1Cp1.Pn = m0Cp1.Pn;
							m1Cp1.Pt = m0Cp1.Pt;
							m1Cp1.matched = true;
						}
					}
					else
					if (k0 == 2)
					{
						/* match one new contact against two old contacts */
						
						var newKey:int = m1Cp1.id.key;
						
						//compare new1 -> old1
						if (newKey == m0Cp1.id.key)
						{
							m1Cp1.Pn = m0Cp1.Pn;
							m1Cp1.Pt = m0Cp1.Pt;
							m1Cp1.matched = true;
						}
						else
						//compare new1 -> old2
						if (newKey == m0Cp2.id.key);
						{
							m1Cp1.Pn = m0Cp2.Pn;
							m1Cp1.Pt = m0Cp2.Pt;
							m1Cp1.matched = true;
						}
					}
				}
				else
				if (k1 == 2)
				{
					if (k0 == 1)
					{
						/* match two new contacts against one old contact */
						
						var oldKey:int = m0Cp1.id.key;
						
						//compare new1 -> old1
						if (m1Cp1.id.key == oldKey)
						{
							m1Cp1.Pn = m0Cp1.Pn;
							m1Cp1.Pt = m0Cp1.Pt;
							m1Cp1.matched = true;
						}
						else
						//compare new2 -> old1
						if (m1Cp2.id.key == oldKey)
						{
							m1Cp2.Pn = m0Cp1.Pn;
							m1Cp2.Pt = m0Cp1.Pt;
							m1Cp2.matched = true;
						}
					}
					else
					if (k0 == 2)
					{
						/* match two new contacts against two old contact */
						
						/* new1 -> old1 & old2 */
						
						//compare new1 -> old1
						if (m1Cp1.id.key == m0Cp1.id.key)
						{
							m1Cp1.Pn = m0Cp1.Pn;
							m1Cp1.Pt = m0Cp1.Pt;
							m1Cp1.matched = true;
							
							//compare new2 -> old2
							if (m1Cp2.id.key == m0Cp2.id.key)
							{
								m1Cp2.Pn = m0Cp2.Pn;
								m1Cp2.Pt = m0Cp2.Pt;
								m1Cp2.matched = true;
								return;
							}
						}
						else
						{
							//compare new1 -> old2
							if (m1Cp1.id.key == m0Cp2.id.key)
							{
								m1Cp1.Pn = m0Cp2.Pn;
								m1Cp1.Pt = m0Cp2.Pt;
								m1Cp1.matched = true;
								
								//compare new2 -> old1
								if (m1Cp2.id.key == m0Cp1.id.key)
								{
									m1Cp2.Pn = m0Cp1.Pn;
									m1Cp2.Pt = m0Cp1.Pt;
									m1Cp2.matched = true;
									return;
								}
							}
						}
						
						/* new2 -> old1 & old2 */
						
						//compare new2 -> old1
						if (m1Cp2.id.key == m0Cp1.id.key)
						{
							m1Cp2.Pn = m0Cp1.Pn;
							m1Cp2.Pt = m0Cp1.Pt;
							m1Cp2.matched = true;
						}
						else
						{
							//compare new2 -> old2
							if (m1Cp2.id.key == m0Cp2.id.key)
							{
								m1Cp2.Pn = m0Cp2.Pn;
								m1Cp2.Pt = m0Cp2.Pt;
								m1Cp2.matched = true;
							}
						}
					}
				}
			}
			//}
		}
		
		override public function getConstructor():Class
		{
			return PolyContact;	
		}
	}
}